import math

TORAD = math.pi/180  # 角度制 -> 弧度制
TOANG = 180/math.pi  # 弧度制 -> 角度制
ERADIUS = 6378.137   # 地球赤道半径
PRADIUS = 6356.725   # 极半径

class GNC_COORDINATE:
    def __init__(self):
        pass

    def get_ec(self, origin_lon, origin_lat):
        ec = PRADIUS + (ERADIUS - PRADIUS) * (90 - origin_lat) / 90
        return ec

    def asin_(self, a):
        if a > 1:
            a = 1
        elif a < -1:
            a = -1
        return math.asin(a)

    def N_trans_to_Lat(self, N, origin_lon, origin_lat):
        ec = self.get_ec(origin_lon, origin_lat)
        lat = 1 / (ec * TORAD * 1000)
        lat = origin_lat + N * lat
        return lat

    def E_trans_to_Lon(self, E, origin_lon, origin_lat):
        ec = self.get_ec(origin_lon, origin_lat)
        lon = 1 / (ec * math.cos(origin_lat*TORAD) * TORAD * 1000)
        lon = origin_lon + E * lon
        return lon

    def Lat_trans_to_N(self, lon, lat, origin_lon, origin_lat):
        step1 = (lat - origin_lat) * TORAD
        step2 = math.sin(step1 / 2.0)
        step3 = pow(step2, 2)
        step4 = self.asin_(math.sqrt(step3))
        step5 = 1000 * 2.0 * step4 * ERADIUS
        if lat < origin_lat:
            step5 *= -1
        return step5

    def Lon_trans_to_E(self, lon, lat, origin_lon, origin_lat):
        step1 = (lon - origin_lon) * TORAD
        step2 = math.sin(step1 / 2.0)
        step3 = math.cos(lat * TORAD)
        step4 = pow(step2, 2) * pow(step3, 2)
        step5 = self.asin_(math.sqrt(step4))
        step6 = 1000 * 2.0 * step5 * ERADIUS
        if lon < origin_lon:
            step6 *= -1
        return step6

    def NED_trans_to_body_x(self, delta_N, delta_E, yaw):
        x = delta_E * math.sin(yaw * TORAD) + delta_N * math.cos(yaw * TORAD)
        return x

    def NED_trans_to_body_y(self, delta_N, delta_E, yaw):
        y = delta_E * math.cos(yaw * TORAD) - delta_N * math.sin(yaw * TORAD)
        return y

